/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写
 ***************************************************************************/

#include "belttrack.h"
#include "visiontrack.h"
#include "coordinatemanager.h"
#include "regdata.h"
#include "filteringalgorithm.h"
#include "positionmixermanager.h"

#define D_BELT_PR_1_INDEX 49
#define D_BELT_PR_2_INDEX 50
#define D_BELT_TRACK_POINT_COUNT_R_INDEX 71 //+beltIndex 区分不同跟踪号
#define D_TRACK_USER_COORDINATE_INDEX 50
//#define D_TRACK_SET_USER_COORDINATE_INDEX 49
#define D_MAX_TRACK_CATCH_SIZE 100
#define D_TRANSITION_BY_DISTANCE 0

BeltTrack::BeltTrack(int beltIndexIn,RegData *regDataIn, BeltTrackConfig configIn, MotionMessage *motionMessageIn,
                     CoordinateManager *coordinateManagerIn, double deltaTimeIn, QObject *parent) : QObject(parent)
{
    beltIndex=beltIndexIn;
    coordinateManager=coordinateManagerIn;
    beltConfig=configIn;
    regData=regDataIn;
    latchEncoderValue_old=0;
    loopCount=0;
    robotId=1;
    debugFlag=0;
//    visionTrack=NULL;
//    trackType=0;

    latchValueForCoordinate=0;
    motionMessage=motionMessageIn;
    deltaTime=deltaTimeIn;
    isTracking=false;

    originalUserCoordinateIndex=0;

    moveStage=E_MOVE_STAGE_NONE;
    moveCommand=E_MOVE_COMMAND_NONE;

    filter_x=new FilteringAlgorithm;
    filter_y=new FilteringAlgorithm;
    positionMixer=new PositionMixerManager(deltaTimeIn);
    currentFrame=new Frame;

    reset();

    initialByConfig();

//    testMovedDistance();


}

int BeltTrack::setCurrentFrameCommand(Frame frameIn)
{
    *currentFrame=frameIn;
    return 1;
}

int BeltTrack::addCeaseMotion(Frame startFrameIn, Frame endFrameIn)
{
    return positionMixer->addCeaseMotion(startFrameIn,endFrameIn,beltVelocity);
}

int BeltTrack::moveOneStep_FrameCommand(Frame &frameOut,double &durationLeftOut)
{
    return positionMixer->moveOneStep_FrameCommand(frameOut,durationLeftOut);
}

double BeltTrack::getBeltTransmission()
{
    return beltConfig.beltTransimission;
}

int BeltTrack::getTrackTransionDistance_start(double &deltaX, double &deltaY)
{
    deltaX=transionDistance_x_start;
    deltaY=transionDistance_y_start;
    return 1;
}

int BeltTrack::getTrackTransionDistance_end(double &deltaX, double &deltaY)
{
    deltaX=transionDistance_x_end;
    deltaY=transionDistance_y_end;
    return 1;
}

int BeltTrack::setDebugFlag(int flagIn)
{
    debugFlag=flagIn;
    return 1;
}

int BeltTrack::recordLatchValue()
{
    usingMutex.lock();
    currentEncoderLatchValue=currentEncoderValue;

    //锁存队列更新：mutex好像会影响实时性，所以该代码块放入此处了。
//    if(currentEncoderLatchValue!=latchEncoderValue_old && D_LOOP_WAIT==loopCount)
//    {
        int tmpLatch=currentEncoderLatchValue;
        latchEncoderValueList.enqueue(tmpLatch);
        if(latchEncoderValueList.size()>D_MAX_TRACK_CATCH_SIZE)
        {
            latchEncoderValueList.dequeue();
            addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22130, robotId,D_MAX_TRACK_CATCH_SIZE);
//            qDebug()<<"warnning ,,,,,latchEncoderValueList.size()>D_MAX_TRACK_CATCH_SIZE"<<D_MAX_TRACK_CATCH_SIZE;
        }
//        qDebug()<<"good latchValue"<<latchEncoderValueList<<"backValue"<<latchEncoderValueList.back()
//                  <<"frontValue"<<latchEncoderValueList.front();
//    }
    latchEncoderValue_old=currentEncoderLatchValue;
//    debugInfo.objectSize=latchEncoderValueList.size();
    usingMutex.unlock();
    return 1;
}

int BeltTrack::getEncoderDeviceType(int &deviceTypeOut, int &aliasOut,int &encoderIndexOut)
{
    deviceTypeOut=beltConfig.encoderDeviceType;
    aliasOut=beltConfig.encoderDeviceAlias;
    encoderIndexOut=beltConfig.encoderIndexOnDevice;
    return 1;
}

int BeltTrack::getDoType(int &doDeviceTypeOut, int &doIndexOut)
{
    doDeviceTypeOut=beltConfig.encoderDeviceType;
    doIndexOut=beltConfig.doIndexOnDevice;
    return 1;
}

int BeltTrack::setTrackOffset(double offset_x, double offset_y)
{
    qDebug()<<"setTrackOffset"<<offset_x<<offset_y;
    beltConfig.trackPointOffset_x=offset_x;
    beltConfig.trackPointOffset_y=offset_y;
    return 1;
}

int BeltTrack::addVisionObject( VisionObjectInfo &visionObjectIn)
{
    usingMutex.lock();

    //判断是否时重合点
    int tmpSize=visionPointBuffer.size();
    QVector<double> tmpXList,tmpYList;
    for(int i=0;i<tmpSize;i++)
    {
        double tmpLength1=getActualMovedDistance(visionPointBuffer[i].latch_encoderValue,currentEncoderValue);
        double actX1,actY1;
        calculateMovedPoint(visionPointBuffer[i].pos_x/1000.0,visionPointBuffer[i].pos_y/1000.0,
                            tmpLength1,beltMoveOrientation,actX1,actY1);
        tmpXList.append(actX1);
        tmpYList.append(actY1);
    }
    //新位置
    double newX,newY;
    double tmpLength1=getActualMovedDistance(visionObjectIn.latch_encoderValue,currentEncoderValue);
    calculateMovedPoint(visionObjectIn.pos_x/1000.0,visionObjectIn.pos_y/1000.0,
                        tmpLength1,beltMoveOrientation,newX,newY);



    bool isSame=false;
    for(int j=0;j<tmpSize;j++)
    {

        double tmpDistance=sqrt((newX-tmpXList[j])*(newX-tmpXList[j])+
                                (newY-tmpYList[j])*(newY-tmpYList[j]));
        if(tmpDistance<beltConfig.objectMininumDistance)
        {
            isSame=true;
        }
    }

    if(false==isSame)
    {
        visionObjectIn.isQualified=false;
        visionPointBuffer.enqueue(visionObjectIn);
        qDebug()<<"sucess:the new point!!!";

    }
    else
    {
        qDebug()<<"ignor:the same point!!!";
    }


    usingMutex.unlock();
    return 1;
}


int BeltTrack::getCurrentEncoderValue()
{
    return currentEncoderValue;
}

//int BeltTrack::getCurrentEncoderLatchValue()
//{
//    return currentEncoderLatchValue;
//}

int BeltTrack::reset()
{
    moveStage=E_MOVE_STAGE_NONE;
    moveCommand=E_MOVE_COMMAND_NONE;
    visionPointBuffer.clear();
    latchEncoderValueList.clear();
    velocityTick=0;
//    startPose_x=0;
//    startPose_y=0;
    currentTransitionTimeAccumued=0;
    positionMixer->reset();
    return 1;
}

int BeltTrack::cease()
{
    velocityTick=-1;
    return 1;
}

int BeltTrack::setOriginalToolCoordinateIndex(int indexIn)
{
    originalUserCoordinateIndex=indexIn;
    return 1;
}

//int BeltTrack::setTrackByVision(VisionTrack *trackIn)
//{
//    visionTrack=trackIn;
//    trackType=1;
//}

int BeltTrack::getBeltTrackDebugInfo(BeltTrackDebugInfo &beltTrackInfoOut)
{
    beltTrackInfoOut=debugInfo;
    return 1;
}

int BeltTrack::modifyBeltTrackConfig(BeltTrackConfig configIn)
{
    beltConfig=configIn;
    initialByConfig();
    return 1;
}



int BeltTrack::trackStart()
{
    qDebug()<<"BeltTrack::trackStart";
    usingMutex.lock();
    filter_x->resetAll();
    filter_y->resetAll();
    //todo 设置位置环的相应刚度。
//    coordinateManager->activateUserCoordinate(originalToolCoordinateIndex);

    //修改用户坐标系沿着皮带方向偏移。coredump风险.//要求皮带运动方向与用户坐标系x方向相同。
    double tmpLength=getActualMovedDistance(latchValueForCoordinate);
    coordinateManager->copyUserCoordinate(originalUserCoordinateIndex,D_TRACK_USER_COORDINATE_INDEX+beltIndex,
                             tmpLength*cos(beltMoveOrientation)+beltConfig.trackPointOffset_x,
                             tmpLength*sin(beltMoveOrientation)+beltConfig.trackPointOffset_y,beltConfig.trackPointOffset_z);

    qDebug()<<"debug::::::tmpLength"<<tmpLength;
    if(tmpLength>100)
    {
        usingMutex.unlock();
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22132, robotId,tmpLength);
        qDebug()<<"debug::::::latchValueForCoordinate"<<latchValueForCoordinate
               <<"currentEncoderValue"<<currentEncoderValue<<"beltConfig.beltTransimission"<<beltConfig.beltTransimission;
        return -1;
    }
    coordinateManager->activateUserCoordinate(D_TRACK_USER_COORDINATE_INDEX+beltIndex);

    moveCommand=E_MOVE_COMMAND_GOMOVE;
    isTracking=true;
//    acceleration=1000.0*beltVelocity/timeConsume_start;
    //过渡开始参数计算
    if(D_TRANSITION_BY_DISTANCE)
    {
        acceleration_start=1000.0*beltVelocity*beltVelocity/(2*beltConfig.trackTransition_startDistance);
        timeConsume_start=1000.0*beltVelocity/acceleration_start;
        transionDistance_x_start=0.5*acceleration_start*timeConsume_start*timeConsume_start*cos(beltMoveOrientation);
        transionDistance_y_start=0.5*acceleration_start*timeConsume_start*timeConsume_start*sin(beltMoveOrientation);

    }
    else
    {
        timeConsume_start=beltConfig.trackTransition_startDistance;
        acceleration_start=1000.0*beltVelocity/timeConsume_start;
        transionDistance_x_start=0.5*acceleration_start*timeConsume_start*timeConsume_start*cos(beltMoveOrientation);
        transionDistance_y_start=0.5*acceleration_start*timeConsume_start*timeConsume_start*sin(beltMoveOrientation);
    }

    qDebug()<<"start transition distance "<<0.5*acceleration_start*timeConsume_start*timeConsume_start
           <<"acceleration_start"<<acceleration_start<<"timeConsume_start"<<timeConsume_start
             <<"beltVelocity"<<beltVelocity;
//    //过渡结束参数计算
//    acceleration_end=1000.0*beltVelocity*beltVelocity/(2*beltConfig.trackTransition_endDistance);
//    timeConsume_end=1000.0*beltVelocity/acceleration_end;
//    transionDistance_x_end=0.5*acceleration_end*timeConsume_end*timeConsume_end*cos(beltMoveOrientation);
//    transionDistance_y_end=0.5*acceleration_end*timeConsume_end*timeConsume_end*sin(beltMoveOrientation);


    if(transionDistance_x_start>100)
    {
        usingMutex.unlock();
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22132, robotId,tmpLength);
        qDebug()<<"debug::::::latchValueForCoordinate222"<<latchValueForCoordinate
               <<"currentEncoderValue"<<currentEncoderValue<<"beltConfig.beltTransimission"<<beltConfig.beltTransimission;
        return -1;
    }

    usingMutex.unlock();
    if(D_TRANSITION_WAIT)
    {
            usleep(8000);//等待坐标系生效.需要放在坐标系修改后，放在算法前后的坐标系不一致，导致突变。

            while(moveStage!=E_MOVE_STAGE_MOVE_HOLDING)
            {
                usleep(8000);
                if(E_MOVE_STAGE_NONE==moveStage)
                {
                    break;
                }
            }
    }


    return 1;
}

int BeltTrack::trackEnd()
{
    qDebug()<<"BeltTrack::trackEnd";
    //过渡结束参数计算
    if(D_TRANSITION_BY_DISTANCE)
    {
        acceleration_end=1000.0*beltVelocity*beltVelocity/(2*beltConfig.trackTransition_endDistance);
        timeConsume_end=1000.0*beltVelocity/acceleration_end;
        transionDistance_x_end=0.5*acceleration_end*timeConsume_end*timeConsume_end*cos(beltMoveOrientation);
        transionDistance_y_end=0.5*acceleration_end*timeConsume_end*timeConsume_end*sin(beltMoveOrientation);
    }
    else
    {
        timeConsume_end=beltConfig.trackTransition_endDistance;
        acceleration_end=1000.0*beltVelocity/timeConsume_end;
        transionDistance_x_end=0.5*acceleration_end*timeConsume_end*timeConsume_end*cos(beltMoveOrientation);
        transionDistance_y_end=0.5*acceleration_end*timeConsume_end*timeConsume_end*sin(beltMoveOrientation);
    }
    qDebug()<<"end transition distance "<<0.5*acceleration_end*timeConsume_end*timeConsume_end;


    if(0)
    {

        if(D_TRANSITION_WAIT)
        {

        }
        else
        {
            usleep(8000);//等待坐标系生效.需要放在坐标系修改后，放在算法前后的坐标系不一致，导致突变。

            while(moveStage!=E_MOVE_STAGE_MOVE_HOLDING)
            {
                usleep(8000);
                if(E_MOVE_STAGE_NONE==moveStage)
                {
                    break;
                }
            }
        }

        usleep(8000);//等待坐标系生效.需要放在坐标系修改后，放在算法前后的坐标系不一致，导致突变。
        moveCommand=E_MOVE_COMMAND_STOPMOVE;
        while(moveStage!=E_MOVE_STAGE_NONE)
        {
            usleep(8000);
            if(E_MOVE_STAGE_NONE==moveStage)
            {
                break;
            }
        }
    }
    else
    {
            Frame positionFrameCommand_end;
            positionFrameCommand_end.p[0]=(*currentFrame).p[0]+transionDistance_x_end/1000.0;
            positionFrameCommand_end.p[1]=(*currentFrame).p[1]+transionDistance_y_end/1000.0;
            positionFrameCommand_end.p[2]=(*currentFrame).p[2];
            positionFrameCommand_end.M=(*currentFrame).M;
            //转换为世界frame
            Frame frameWorld1,frameWorld2;
            frameWorld1=coordinateManager->userFrameToWorldFrame(*currentFrame);
            frameWorld2=coordinateManager->userFrameToWorldFrame(positionFrameCommand_end);



            //规划过渡轨迹
            positionMixer->addCeaseMotion(frameWorld1,frameWorld2,beltVelocity);

//            double sumDistance1=sqrt(((*currentFrame).p[0]-positionFrameCommand_end.p[0])*((*currentFrame).p[0]-positionFrameCommand_end.p[0])+
//                    ((*currentFrame).p[1]-positionFrameCommand_end.p[1])*((*currentFrame).p[1]-positionFrameCommand_end.p[1])+
//                    ((*currentFrame).p[2]-positionFrameCommand_end.p[2])*((*currentFrame).p[2]-positionFrameCommand_end.p[2]));

//            qDebug()<<"addCeaseMotion---sumDistance1"<<sumDistance1
//                  <<"currentFrame"<<(*currentFrame).p[0]<<(*currentFrame).p[1]<<(*currentFrame).p[2]
//                    <<"positionFrameCommand_end"<<positionFrameCommand_end.p[0]<<positionFrameCommand_end.p[1]<<positionFrameCommand_end.p[2];

    }

    isTracking=false;
    //    更换追踪物体需要把用户坐标系还原。
    coordinateManager->equalUserCoordinate(originalUserCoordinateIndex,D_TRACK_USER_COORDINATE_INDEX+beltIndex);
    coordinateManager->restoreUserCoordinate();
    moveCommand=E_MOVE_COMMAND_NONE;
    moveStage=E_MOVE_STAGE_NONE;
    return 1;
}

int BeltTrack::getTrackData()
{
    qDebug()<<"BeltTrack::getTrackData";
    switch(beltConfig.trackFunction)
    {
    case 0:
    {
        return 0;
    }
    case 1:
    {
        usingMutex.lock();
        if(debugInfo.okSize>0)//visionPointBuffer.size()
        {
            VisionObjectInfo tmpObject;
            getBestPoint(visionPointBuffer,tmpObject);
            regData->setPrRegValueType(D_BELT_PR_1_INDEX,true);
            regData->setPrIJValue(D_BELT_PR_1_INDEX,0,tmpObject.pos_x);
            regData->setPrIJValue(D_BELT_PR_1_INDEX,1,tmpObject.pos_y);
            regData->setPrIJValue(D_BELT_PR_1_INDEX,5,tmpObject.pos_r);
            regData->setPrRegValueType(D_BELT_PR_2_INDEX,true);
            regData->setPrIJValue(D_BELT_PR_2_INDEX,0,tmpObject.pos_x);
            regData->setPrIJValue(D_BELT_PR_2_INDEX,1,tmpObject.pos_y);
            regData->setPrIJValue(D_BELT_PR_2_INDEX,5,tmpObject.pos_r);
            latchValueForCoordinate=tmpObject.latch_encoderValue;
        }
        else
        {
            usingMutex.unlock();
            return -1;
        }

        usingMutex.unlock();
        return 1;
    }
    case 2:
    {
        usingMutex.lock();
        if(latchEncoderValueList.size()>0)
        {
            latchValueForCoordinate=latchEncoderValueList.dequeue();
        }
        usingMutex.unlock();

        return 1;
    }
    }
    return 1;


}

int BeltTrack::loopCommontimeOnce()
{



    return 1;
}

#define D_LOOP_WAIT 10
int BeltTrack::diSensorLoopLogic()
{

    usingMutex.lock();


    if(0==latchEncoderValueList.size())
    {
//        qDebug()<<"no latch value";
        debugInfo.okSize=0;
        regData->setRRegValue(D_BELT_TRACK_POINT_COUNT_R_INDEX+beltIndex,0);
        usingMutex.unlock();
        return 0;
    }

    //清除已经超过的数据
    deleteExceedPoint_di();


    //筛选重合点
    deleteOverridePoint_di();


    //合格数量
    debugInfo.okSize=getQualifiedPointNum_di();
    regData->setRRegValue(D_BELT_TRACK_POINT_COUNT_R_INDEX+beltIndex,debugInfo.okSize);

    usingMutex.unlock();

    return 1;
}

int BeltTrack::visionSensorLoopLogic()
{

    usingMutex.lock();
    debugInfo.objectSize=visionPointBuffer.size();
    if(0==visionPointBuffer.size())
    {
//        qDebug()<<"visionSensorLoopLogic ,belt receive no visionPoint";
        debugInfo.okSize=0;
        regData->setRRegValue(D_BELT_TRACK_POINT_COUNT_R_INDEX+beltIndex,0);
        usingMutex.unlock();
        return 0;
    }

    //清除已经超过范围的数据
    deleteExceedPoint();


    //计算合格点数量
    debugInfo.okSize=getQualifiedPointNum();
    regData->setRRegValue(D_BELT_TRACK_POINT_COUNT_R_INDEX+beltIndex,debugInfo.okSize);
//    qDebug()<<"currentLength"<<currentLength<<" minTrackOk"<<minTrackOk
//           <<" maxTrackOk"<<maxTrackOk;

    usingMutex.unlock();

    return 1;

}


#define D_VELOCITY_SMOOTH 30
int BeltTrack::loopRealtimeOnce(int &encoderData, PointPro &deltaPoseOut, E_MOVE_STAGE &stageOut)
{
    stageOut=moveStage;
    bool isInHoldingStatusOut=false;
    if(500==debugFlag)
    {
        qDebug()<<"BeltTrack::loopRealtimeOnce beltIndex"<<beltIndex<<encoderData;
    }
    if(0==beltConfig.trackFunction)
    {

        return 0;
    }
    if(loopCount<D_LOOP_WAIT)
    {
        loopCount++;
    }



    currentEncoderValue=encoderData;


    //皮带速度计算
    if(-1==velocityTick)//暂停后需要重新复位。
    {
        velocityTick=0;
        currentEncoderValue_old=currentEncoderValue;
    }
    else if(velocityTick<D_VELOCITY_SMOOTH)
    {
        velocityTick++;
    }
    else if(D_VELOCITY_SMOOTH==velocityTick)
    {
        double tmpMovedLength=getActualMovedDistance(currentEncoderValue_old,currentEncoderValue);
        beltVelocity=tmpMovedLength/(deltaTime*D_VELOCITY_SMOOTH);
        if(beltVelocity>10000)
        {

//            addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "BeltTrackManager", 22132, robotId,tmpLength);
            qDebug()<<"debug::::::beltVelocity>10000 beltVelocity"<<beltVelocity
                   <<"currentEncoderValue"<<currentEncoderValue<<"currentEncoderValue_old"<<currentEncoderValue_old
                  <<"beltConfig.beltTransimission"<<beltConfig.beltTransimission;

        }
        velocityTick=0;
        currentEncoderValue_old=currentEncoderValue;
    }


    //latchValueForCoordinate=0, currentValue=-1104445时长度为负数
    double tmpLength=getActualMovedDistance(latchValueForCoordinate);
    if(505==debugFlag)
    {
        qDebug()<<"505==debugFlag"<<latchValueForCoordinate
               <<"currentEncoderValue"<<currentEncoderValue<<"beltConfig.beltTransimission"<<beltConfig.beltTransimission;
        return -1;
    }
    if(isTracking)
    {

          coordinateManager->copyUserCoordinate(originalUserCoordinateIndex,D_TRACK_USER_COORDINATE_INDEX+beltIndex,
                                     tmpLength*cos(beltMoveOrientation)+beltConfig.trackPointOffset_x,
                                     tmpLength*sin(beltMoveOrientation)+beltConfig.trackPointOffset_y,
                                                beltConfig.trackPointOffset_z);

    }

    deltaPoseOut.positionValue.resize(6);
    if(false==moveTransitionLoop(deltaPoseOut))
    {
        if(moveStage==E_MOVE_STAGE_MOVE_HOLDING)
        {
//            deltaPoseOut.positionValue[0]=tmpLength*cos(beltMoveOrientation)*1000.0;
//            deltaPoseOut.positionValue[1]=tmpLength*sin(beltMoveOrientation)*1000.0;
//            deltaPoseOut.positionValue[2]=0;

            deltaPoseOut.positionValue[0]=0;
            deltaPoseOut.positionValue[1]=0;
            deltaPoseOut.positionValue[2]=0;
            isInHoldingStatusOut=true;
        }
        else
        {
            deltaPoseOut.positionValue[0]=0;
            deltaPoseOut.positionValue[1]=0;
            deltaPoseOut.positionValue[2]=0;
        }

    }





    if(500==debugFlag)
    {
        qDebug()<<"BeltTrack::loopRealtimeOnce currentMovedLength "<<tmpLength
               <<"tmpLength*cos(beltMoveOrientation)"<<tmpLength*cos(beltMoveOrientation)
                 <<"tmpLength*sin(beltMoveOrientation)"<<tmpLength*sin(beltMoveOrientation);
    }

    debugInfo.currentMovedLength=tmpLength;
    debugInfo.movingLength=tmpLength;

    if(500==debugFlag)
    {
        qDebug()<<"BeltTrack::loopRealtimeOnce movingLength "<<debugInfo.movingLength;
    }

    //logic loop

    stageOut=moveStage;


    //放在非实时，会导致更新频率太慢，合格数量不能及时发布，导致wait和获取数据不同步，有问题。
    debugInfo.isTracking=isTracking;
    debugInfo.beltVelocity=beltVelocity;
    switch(beltConfig.trackFunction)
    {
    case 0:
    {
        return 0;
    }
    case 1:
    {
        return visionSensorLoopLogic();
    }
    case 2:
    {
        return diSensorLoopLogic();
    }
    }

    return 1;

}

int BeltTrack::getEncoderIndex()
{
    return beltConfig.encoderIndexOnDevice;

}
//poseFeedback 单位mm
int BeltTrack::moveTransitionLoop( PointPro &deltaPoseOut)
{

//    qDebug()<<"moveStage"<<moveStage;
    debugInfo.moveStage=moveStage;
    bool isMoveValidOut=false;


    deltaPoseOut.positionValue.resize(6);

    //状态切换
    switch (moveStage)
    {
    case E_MOVE_STAGE_NONE:
    {
        if(E_MOVE_COMMAND_GOMOVE==moveCommand)
        {
            moveStage=E_MOVE_STAGE_GOMOVE_BEGIN;
            currentTransitionTimeAccumued=0;
            deltaPoseOut.positionValue[0]=transionDistance_x_start;
            deltaPoseOut.positionValue[1]=transionDistance_y_start;
            isMoveValidOut=true;

        }

        break;
    }
    case E_MOVE_STAGE_GOMOVE_BEGIN:
    {
        currentTransitionTimeAccumued+=deltaTime;
        //u=-a*t
//        poseCommandOut.positionValue[0]=startPose+0.5*acceleration*timeConsume_start*timeConsume_start-
//                                                   0.5*acceleration*(timeConsume_start-timeAccumued)*
//                                                    (timeConsume_start-timeAccumued);
        //导致立马就合成速度为皮带速度。
//        deltaPoseOut.positionValue[0]=startPose_x+0.5*acceleration_start*currentTransitionTimeAccumued*currentTransitionTimeAccumued*cos(beltMoveOrientation)
//                -transionDistance_x_start;
//        deltaPoseOut.positionValue[1]=startPose_y+0.5*acceleration_start*currentTransitionTimeAccumued*currentTransitionTimeAccumued*sin(beltMoveOrientation)
//                -transionDistance_y_start;

            double tmpAcc1=acceleration_start*cos(beltMoveOrientation);
            deltaPoseOut.positionValue[0]=0.5*tmpAcc1*(timeConsume_start-currentTransitionTimeAccumued)*
                                                        (timeConsume_start-currentTransitionTimeAccumued);

            double tmpAcc2=acceleration_start*sin(beltMoveOrientation);
            deltaPoseOut.positionValue[1]=0.5*tmpAcc2*(timeConsume_start-currentTransitionTimeAccumued)*
                                                        (timeConsume_start-currentTransitionTimeAccumued);

//        double tmpDelta=timeAccumued*timeAccumued-(timeAccumued-deltaTime)*(timeAccumued-deltaTime);
//        deltaPoseOut.positionValue[0]=0.5*acceleration*tmpDelta*cos(beltMoveOrientation);
//        deltaPoseOut.positionValue[1]=startPose_y+0.5*acceleration*tmpDelta*sin(beltMoveOrientation);
        if(timeConsume_start<currentTransitionTimeAccumued)
        {
            deltaPoseOut.positionValue[0]=0;
            deltaPoseOut.positionValue[1]=0;
            moveStage=E_MOVE_STAGE_MOVE_HOLDING;

        }
        isMoveValidOut=true;
        if(0)
        {
            qDebug()<<"E_MOVE_STAGE_GOMOVE_BEGIN"<<deltaPoseOut.positionValue[0]<<deltaPoseOut.positionValue[1];
        }

        break;
    }
    case E_MOVE_STAGE_MOVE_HOLDING:
    {
        if(E_MOVE_COMMAND_STOPMOVE==moveCommand)
        {
            moveStage=E_MOVE_STAGE_STOPMOVE_BEGIN;
//            startPose_x=0;
//            startPose_y=0;
//            acceleration=1000.0*beltVelocity/timeConsume_start;
            currentTransitionTimeAccumued=0;

        }

        break;
    }
    case E_MOVE_STAGE_STOPMOVE_BEGIN:
    {


        if(0)
        {
            currentTransitionTimeAccumued+=deltaTime;

            //距离不对
            deltaPoseOut.positionValue[0]=-0.5*acceleration_end*currentTransitionTimeAccumued*currentTransitionTimeAccumued*cos(beltMoveOrientation);
            deltaPoseOut.positionValue[1]=-0.5*acceleration_end*currentTransitionTimeAccumued*currentTransitionTimeAccumued*sin(beltMoveOrientation);

            if(timeConsume_end<currentTransitionTimeAccumued)
            {
                deltaPoseOut.positionValue[0]=0;
                deltaPoseOut.positionValue[1]=0;
                moveStage=E_MOVE_STAGE_NONE;

            }
            else
            {
                if(1)
                {
                    qDebug()<<"E_MOVE_STAGE_STOPMOVE_BEGIN"<<deltaPoseOut.positionValue[0]<<deltaPoseOut.positionValue[1];
                }
    //            isMoveValidOut=true;
                isMoveValidOut=false;
            }
        }
        else
        {
            moveStage=E_MOVE_STAGE_NONE;
        }

        break;
    }


    default:
        break;
    }



    debugInfo.isMoveValidOut=isMoveValidOut;
    debugInfo.poseFeedback=0;
    debugInfo.acceleration=acceleration_start;

    return isMoveValidOut;

}




#define D_2POW30 1073741824
#define D_2POW32 4294967296

double BeltTrack::getActualMovedDistance(int oldValue, int newValue)
{
    //允许编码器每次最大变化量2^30
    long long count=0;
    long long deltaMovePulse_long;
    long long newLeng=newValue;
    long long oldLeng=oldValue;
    long long deltaMovePulse=newLeng-oldLeng;
    if(deltaMovePulse>D_2POW30)
    {
        count--;
//        qDebug()<<"BeltTrack::getActualMovedDistance int 溢出１, newValue"<<newValue<<" oldValue"<<oldValue
//                  <<" deltaMovePulse"<<deltaMovePulse;
    }
    else if(deltaMovePulse<-D_2POW30)
    {
        count++;
//        qDebug()<<"BeltTrack::getActualMovedDistance int 溢出2, newValue"<<newValue<<" oldValue"<<oldValue
//                  <<" deltaMovePulse"<<deltaMovePulse;
    }
    else
    {

    }
    deltaMovePulse_long=count*D_2POW32+deltaMovePulse;
    deltaMovePulse=deltaMovePulse_long;
    if(0!=count)
    {
//        qDebug()<<"count"<<count<<" deltaMovePulse_long"<<deltaMovePulse_long<<" deltaMovePulse"<<deltaMovePulse;
    }

    double tmpMovedLength=deltaMovePulse*(beltConfig.beltTransimission+beltConfig.beltTransimissionAdjust)*beltConfig.encoderDirection;
//    qDebug()<<"tmpMovedLength"<<tmpMovedLength<<" deltaMovePulse"<<deltaMovePulse
//           <<" beltTransimission"<<beltConfig.beltTransimission<<" encoderDirection"<<beltConfig.encoderDirection;
    return tmpMovedLength;
}

int BeltTrack::getActualMovedPulse(int oldValue, int newValue)
{
    //允许编码器每次最大变化量2^30
    long long count=0;
    long long deltaMovePulse_long;
    long long newLeng=newValue;
    long long oldLeng=oldValue;
    long long deltaMovePulse=newLeng-oldLeng;
    if(deltaMovePulse>D_2POW30)
    {
        count--;
        qDebug()<<"BeltTrack::getActualMovedDistance int 溢出１, newValue"<<newValue<<" oldValue"<<oldValue;
    }
    else if(deltaMovePulse<-D_2POW30)
    {
        count++;
        qDebug()<<"BeltTrack::getActualMovedDistance int 溢出2, newValue"<<newValue<<" oldValue"<<oldValue;
    }
    else
    {

    }
    deltaMovePulse_long=count*D_2POW32+deltaMovePulse;
    deltaMovePulse=deltaMovePulse_long;
    if(0!=count)
    {
        qDebug()<<"count"<<count<<" deltaMovePulse_long"<<deltaMovePulse_long<<" deltaMovePulse"<<deltaMovePulse;
    }

    return deltaMovePulse;
}

int BeltTrack::testMovedDistance()
{
    double len1=getActualMovedDistance(0,10000);
    qDebug()<<"len1"<<len1;
    double len2=getActualMovedDistance(10000,0);
    qDebug()<<"len2"<<len2;

    double len3=getActualMovedDistance(1048576,2147483647);
    qDebug()<<"len3"<<len3;

    double len4=getActualMovedDistance(2147483640,-10000);
    qDebug()<<"len4"<<len4;

    double len5=getActualMovedDistance(-10000,-5000);
    qDebug()<<"len5"<<len5;

}

int BeltTrack::getBestPoint(QQueue<VisionObjectInfo> &visionPointBufferIn,VisionObjectInfo &objectOut)
{
//    objectOut=visionPointBufferIn.dequeue();
//    return 1;
    //通过点到直线距离进行判断

    //全部转换为当前坐标ｘ,y
    int tmpSize=visionPointBufferIn.size();
    QVector<double> tmpXList,tmpYList;
    QVector<int> indexList;
    for(int i=0;i<tmpSize;i++)
    {
        if(visionPointBufferIn[i].isQualified)
        {
            double tmpLength1=getActualMovedDistance(visionPointBufferIn[i].latch_encoderValue,currentEncoderValue);
            double actX1,actY1;
            calculateMovedPoint(visionPointBufferIn[i].pos_x/1000.0,visionPointBufferIn[i].pos_y/1000.0,
                                tmpLength1,beltMoveOrientation,actX1,actY1);
            tmpXList.append(actX1);
            tmpYList.append(actY1);
            indexList.append(i);
        }

    }
    //求直线公式．
    double lineA,lineB,lineC;
    lineA=-tan(beltMoveOrientation+M_PI_2);
    lineB=1;
    lineC=-refY_start-lineA*refX_start;

    //求所有距离
    QVector<double> distanceList;
    tmpSize=indexList.size();
    qDebug()<<"BeltTrack::getBestPoint"<<lineA<<lineB<<lineC<<refX_start<<refY_start;
    for(int i=0;i<tmpSize;i++)
    {
        double tmpDistance=fabs(lineA*tmpXList[i]+lineB*tmpYList[i]+lineC)/
                sqrt(lineA*lineA+lineB*lineB);
        distanceList.append(tmpDistance);
        qDebug()<<"--------------------------"<<tmpDistance;

    }
    if(0==distanceList.size())
    {
        qDebug()<<"error,,,,0==distanceList.size()";
        return -1;
    }
    //选择距离最远的．
    double tmpMax=distanceList[0];
    int bestId=indexList[0];
    for(int i=1;i<tmpSize;i++)
    {
        if(distanceList[i]>tmpMax)
        {
            bestId=indexList[i];
            tmpMax=distanceList[i];
        }

    }
    qDebug()<<"bestid"<<bestId<<"tmpMax"<<tmpMax<<"indexList"<<indexList;

    //队列更新
    QQueue<VisionObjectInfo> tmpBuffer;
    tmpSize=visionPointBufferIn.size();
    for(int i=0;i<tmpSize;i++)
    {
        if(i!=bestId)
        {
            tmpBuffer.enqueue(visionPointBufferIn.dequeue());
        }
        else
        {

            objectOut=visionPointBufferIn.dequeue();
        }

    }
    visionPointBufferIn=tmpBuffer;

    return 1;
}

int BeltTrack::calculateMovedPoint(double xIn, double yIn,double movedLengthIn, double moveOrientation,
                                   double &xOut, double &yOut)
{
    xOut=xIn+movedLengthIn*cos(moveOrientation);
    yOut=yIn+movedLengthIn*sin(moveOrientation);
    return 1;
}

int BeltTrack::initialByConfig()
{
    validTrackStartPoint_fromA=beltConfig.validTrackStartPoint_fromA;
    validTrackEndPoint_fromA=beltConfig.validTrackEndPoint_fromA;

    double tmpLength=sqrt((beltConfig.xValue_referenceAPoint-beltConfig.xValue_referenceBPoint)*
                          (beltConfig.xValue_referenceAPoint-beltConfig.xValue_referenceBPoint)+
                          (beltConfig.yValue_referenceAPoint-beltConfig.yValue_referenceBPoint)*
                          (beltConfig.yValue_referenceAPoint-beltConfig.yValue_referenceBPoint))/1000.0;
    beltConfig.beltTransimission=fabs( tmpLength/
            getActualMovedPulse(beltConfig.encoderValue_referenceAPoint,beltConfig.encoderValue_referenceBPoint) );

    beltMoveOrientation=atan2(beltConfig.yValue_referenceBPoint-beltConfig.yValue_referenceAPoint,
                              beltConfig.xValue_referenceBPoint-beltConfig.xValue_referenceAPoint);

    calculateMovedPoint(beltConfig.xValue_referenceAPoint/1000.0,beltConfig.yValue_referenceAPoint/1000.0,
                        validTrackStartPoint_fromA,beltMoveOrientation,refX_start,refY_start);
    calculateMovedPoint(beltConfig.xValue_referenceAPoint/1000.0,beltConfig.yValue_referenceAPoint/1000.0,
                        validTrackEndPoint_fromA,beltMoveOrientation,refX_end,refY_end);

//    beltConfig.beltTransimission=beltConfig.beltTransimission+beltConfig.beltTransimissionAdjust;
}

double BeltTrack::angleRotationJudgement(double targetAngle, double initialAngle)
{
    double angle1,angle2,angle3;
    angle1=targetAngle-initialAngle;
    angle2=angle1+2*M_PI;
    angle3=angle1-2*M_PI;
    double minNum;
    if(fabs(angle1)<=fabs(angle2))
    {
        minNum=angle1;
    }
    else
    {
        minNum=angle2;
    }

    if(fabs(angle3)<=fabs(minNum))
    {
        minNum=angle3;
    }

    return minNum;

}


int BeltTrack::deleteExceedPoint()
{

    QQueue<VisionObjectInfo> tmpvisionPointBuffer;
    int tmpSize=visionPointBuffer.size();

    for(int i=0;i<tmpSize;i++)
    {
        if(true==visionPointBuffer[i].isQualified)
        {
            double tmpLength=getActualMovedDistance(visionPointBuffer[i].latch_encoderValue,currentEncoderValue);
            double actX,actY;
            calculateMovedPoint(visionPointBuffer[i].pos_x/1000.0,visionPointBuffer[i].pos_y/1000.0,
                                tmpLength,beltMoveOrientation,actX,actY);



            double tmpAngle1=atan2(refY_end-actY,refX_end-actX);
            double angleBias=angleRotationJudgement(tmpAngle1,beltMoveOrientation);

            if(fabs(angleBias)>M_PI_2)
            {
                qDebug()<<"deleteExceedPoint ,angleBias"<<angleBias<<" ,latch value"
                       <<visionPointBuffer[i].latch_encoderValue<<"actX"<<actX<<"actY"<<actY
                         <<"refX"<<refX_end<<"refY"<<refY_end;

            }
            else
            {
                tmpvisionPointBuffer.enqueue(visionPointBuffer[i]);

            }
        }
        else
        {
            tmpvisionPointBuffer.enqueue(visionPointBuffer[i]);
        }
    }
    visionPointBuffer=tmpvisionPointBuffer;

    return 1;
}


int BeltTrack::getQualifiedPointNum()
{
    int tmpSize=visionPointBuffer.size();
    int validPointSize=0;


    for(int i=0;i<tmpSize;i++)
    {
        double tmpLength;
        if(false==visionPointBuffer[i].isQualified)
        {
            tmpLength=getActualMovedDistance(visionPointBuffer[i].latch_encoderValue,currentEncoderValue);
            double actX,actY;
            calculateMovedPoint(visionPointBuffer[i].pos_x/1000.0,visionPointBuffer[i].pos_y/1000.0,
                                tmpLength,beltMoveOrientation,actX,actY);
            double tmpAngle1=atan2(refY_start-actY,refX_start-actX);
            double angleBias=angleRotationJudgement(tmpAngle1,beltMoveOrientation);

            if(fabs(angleBias)>M_PI_2)
            {
                validPointSize++;
                visionPointBuffer[i].isQualified=true;
            }
            if(0==i)
            {
                debugInfo.currentMovedLength=tmpLength;
            }
        }
        else
        {
            validPointSize++;
        }





    }
    return validPointSize;
}

int BeltTrack::deleteExceedPoint_di()
{
    //里面打印影响实时性，因为锁的原因。
    QQueue<int> tmpValueList;

    double tmpLength;
    int tmpSize=latchEncoderValueList.size();
    for(int i=0;i<tmpSize;i++)
    {
        tmpLength=getActualMovedDistance(latchEncoderValueList[i]);
        if(fabs(tmpLength)<validTrackEndPoint_fromA)
        {
            tmpValueList.enqueue(latchEncoderValueList[i]);

        }
        else
        {
//                qDebug()<<"超过跟踪范围，，，tmpLength"<<tmpLength;
        }
    }
    latchEncoderValueList=tmpValueList;
    return 1;
}

int BeltTrack::deleteOverridePoint_di()
{
    if(0==latchEncoderValueList.size())
    {
        return 0;
    }
    QQueue<int> tmpValueList;
    tmpValueList.enqueue(latchEncoderValueList.dequeue());

    double tmpLength;
    int tmpSize=latchEncoderValueList.size();
    for(int i=0;i<tmpSize;i++)
    {
        tmpLength=getActualMovedDistance(tmpValueList.back(),latchEncoderValueList[i]);
        if(fabs(tmpLength)>beltConfig.objectMininumDistance)
        {
            tmpValueList.enqueue(latchEncoderValueList[i]);

        }
        else
        {
                qDebug()<<"检测到重叠，，，tmpLength"<<tmpLength;
        }
    }
    latchEncoderValueList=tmpValueList;
    return 1;

}

int BeltTrack::getQualifiedPointNum_di()
{
    double tmpLength;
    int okNum=0;
    for(int i=0;i<latchEncoderValueList.size();i++)
    {

        tmpLength=getActualMovedDistance(latchEncoderValueList[i]);
        if(fabs(tmpLength)>validTrackStartPoint_fromA && fabs(tmpLength)<validTrackEndPoint_fromA)
        {
            okNum++;
//            qDebug()<<"ok,,,,movedLength"<<tmpLength;

        }
//        qDebug()<<"movedLength"<<tmpLength<<"latch"<<latchEncoderValueList[i]
//                  <<" currentEncoderValue"<<currentEncoderValue;
        if(0==i)
        {
            debugInfo.currentMovedLength=tmpLength;
        }
    }
    return okNum;
}


double BeltTrack::getActualMovedDistance(int latchValue)
{
    return getActualMovedDistance(latchValue,currentEncoderValue);

}

void BeltTrack::addMsg(int messageLevel, std::string componentName, std::string messageType, int messageCode, int robotIdIn, int parameter1, int parameter2)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotIdIn;

    string infomationStr;
    QString tmpStr;

    switch(messageCode)
    {
    case 22132:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(")编码器位移长度(%1)计算错误，请联系厂商！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22131:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(")编码器序号(%1)超过支持区间！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22130:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(")跟踪缓存个数超过限制大小%1,旧缓存被丢弃！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }




        default:
        {
            qDebug()<<"BeltTrackManager::addMsg====not match error code";
            break;
        }
    }

    tmpMsg.MessageInformation = infomationStr;

    #ifndef MOTION_SERVER_UNIT_TEST
    motionMessage->addMsg(tmpMsg);
    #endif
}
